#ifndef __RANDOM_HELPERS_H
#define __RANDOM_HELPERS_H
#include <TooN/TooN.h>
#include <sys/timeb.h>
#include <inttypes.h>
#include <time.h>

const double RAD_2_DEG = 180.0/M_PI;
const double DEG_2_RAD = M_PI/180.0;

using namespace TooN;

inline uint64_t ClockGetTime()
{
  timespec ts;
  clock_gettime(CLOCK_REALTIME, &ts);
  return (uint64_t)ts.tv_sec * 1000000LL + (uint64_t)ts.tv_nsec / 1000LL;
}

inline uint64_t ClockTimeDifference(uint64_t &time1, uint64_t &time2)
{
  if(time1 > time2)
    return time1 - time2;
  else
    return time2 - time1;
}

inline TooN::SO3<> Roll(const double dRoll)
{
  const TooN::Matrix<3,3> R = TooN::Data( 1.0,      0.0,          0.0,
                                          0.0,      cos(dRoll),  -sin(dRoll),
                                          0.0,      sin(dRoll),  cos(dRoll));

  return TooN::SO3<>(R);
}

inline TooN::SO3<> Pitch(const double dPitch)
{
  const TooN::Matrix<3,3> R = TooN::Data( cos(dPitch),  0.0,    sin(dPitch),
                                          0.0,          1.0,    0.0,
                                          -sin(dPitch), 0.0,    cos(dPitch));

  return TooN::SO3<>(R);
}

inline TooN::SO3<> Yaw(const double dYaw)
{
  const TooN::Matrix<3,3> R = TooN::Data( cos(dYaw),  -sin(dYaw),   0.0,
                                          sin(dYaw),  cos(dYaw),    0.0,
                                          0.0,        0.0,          1.0);

  return TooN::SO3<>(R);
}

inline void WrapTo2Pi(float &v)
{
	bool bIsNeg = (v < 0);
	v = fmod(v, (float)(2*M_PI));
	if(bIsNeg) 
    v += 2*M_PI;
}

inline TooN::Vector<3> UnPackSO3_RPY(const TooN::SO3<> &so3)
{
  TooN::Vector<3> v3RPY; 

  v3RPY[1] = -asin(so3.get_matrix()(2,0)); //pitch
  v3RPY[0] = atan2(so3.get_matrix()(2,1)/cos(v3RPY[1]), so3.get_matrix()(2,2)/cos(v3RPY[1])); //roll
  v3RPY[2] = atan2(so3.get_matrix()(1,0)/cos(v3RPY[1]), so3.get_matrix()(0,0)/cos(v3RPY[1])); //yaw

  /* MRPT version ---
	// Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
	v3RPY[1] = atan2(-so3.get_matrix()(2,0), hypot(so3.get_matrix()(0,0),so3.get_matrix()(1,0))); //asin( - so3.get_matrix()(2,0) );

	// Roll:
	if((fabs(so3.get_matrix()(2,1))+fabs(so3.get_matrix()(2,2))) < 10*std::numeric_limits<double>::epsilon())
	{
		//Gimbal lock between yaw and roll. This one is arbitrarily forced to be zero.
		//Check http://babel.isa.uma.es/mrpt/reference/svn/classmrpt_1_1poses_1_1_c_pose3_d.html. If cos(pitch)==0, the homogeneous matrix is:
		//When sin(pitch)==1:
		//  /0  cysr-sycr cycr+sysr x\   /0  sin(r-y) cos(r-y)  x\.
		//  |0  sysr+cycr sycr-cysr y| = |0  cos(r-y) -sin(r-y) y|
		//  |-1     0         0     z|   |-1    0         0     z|
		//  \0      0         0     1/   \0     0         0     1/
		//
		//And when sin(pitch)=-1:
		//  /0 -cysr-sycr -cycr+sysr x\   /0 -sin(r+y) -cos(r+y) x\.
		//  |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y)  -sin(r+y) y|
		//  |1      0          0     z|   |1    0          0     z|
		//  \0      0          0     1/   \0    0          0     1/
		//
		//Both cases are in a "gimbal lock" status. This happens because pitch is vertical.

		v3RPY[0] = 0.0;
		if (v3RPY[1] > 0) 
      v3RPY[2] = atan2(so3.get_matrix()(1,2), so3.get_matrix()(0,2));
		else 
      v3RPY[2] = atan2(-so3.get_matrix()(1,2), -so3.get_matrix()(0,2));
	}
	else
	{
		v3RPY[0] = atan2( so3.get_matrix()(2,1), so3.get_matrix()(2,2) );
		v3RPY[2] = atan2( so3.get_matrix()(1,0), so3.get_matrix()(0,0) );
	}*/

  return v3RPY;
}

inline TooN::SO3<> PackSO3_RPY(const float roll, const float pitch, const float yaw)
{
  const float cyaw = cos(yaw);
  const float syaw = sin(yaw);
  const float croll = cos(roll);
  const float sroll = sin(roll);
  const float cpitch = cos(pitch);
  const float spitch = sin(pitch);
  const TooN::Matrix<3,3> R = TooN::Data( cyaw * cpitch,    -syaw*croll + cyaw*spitch*sroll,    syaw*sroll + cyaw*spitch*croll,
                              syaw*cpitch,      cyaw*croll + syaw*spitch*sroll,     -cyaw*sroll + syaw*spitch*croll,
                              -spitch,          cpitch*sroll,                       cpitch*croll);

  return TooN::SO3<>(R);
}

inline TooN::SO3<> RotMatBfromA(TooN::Vector<3> v3A, TooN::Vector<3> v3B)
{
  // Normalise to unit length
  v3A = unit(v3A);
  v3B = unit(v3B);

  // Get the axis-angle representation
  double dAngle = acos(v3A*v3B); 
  Vector<3> v3Axis = v3A ^ v3B;
  normalize(v3Axis);

  // Exponentiate to get rotation matrix
  return TooN::SO3<>::exp(dAngle*v3Axis);
}

inline TooN::SO3<> YawPitchRotMatBfromA(TooN::Vector<3> v3A, TooN::Vector<3> v3B)
{
  // Normalise to unit length
  v3A = unit(v3A);
  v3B = unit(v3B);

  double dYaw = atan2(v3B[1],v3B[0]) - atan2(v3A[1],v3A[0]);
  TooN::SO3<> so3Yaw = Yaw(dYaw);

  Vector<3> v3ProjA = so3Yaw*v3A;
  
  // Get the axis-angle representation
  double dAngle = acos(v3ProjA*v3B); 
  Vector<3> v3Axis = v3ProjA ^ v3B;
  normalize(v3Axis);

  // Exponentiate to get rotation matrix
  TooN::SO3<> so3Pitch = TooN::SO3<>::exp(dAngle*v3Axis);
  
  return (so3Yaw*so3Pitch);
}


inline TooN::Matrix<3,6> JacobianWposFromCpos(const TooN::SE3<> &se3CfromW)
{
  TooN::Vector<3> v3RPY = UnPackSO3_RPY(se3CfromW.get_rotation());
  TooN::Vector<3> v3T = se3CfromW.get_translation();

  const TooN::Matrix<3,6> J = TooN::Data(
-cos(v3RPY[2])*cos(v3RPY[1]), -sin(v3RPY[2])*cos(v3RPY[1]), sin(v3RPY[1]), 0, cos(v3RPY[2])*sin(v3RPY[1])*v3T[0]+sin(v3RPY[2])*sin(v3RPY[1])*v3T[1]+cos(v3RPY[1])*v3T[2], sin(v3RPY[2])*cos(v3RPY[1])*v3T[0]-cos(v3RPY[2])*cos(v3RPY[1])*v3T[1],

sin(v3RPY[2])*cos(v3RPY[0])-cos(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]), -cos(v3RPY[2])*cos(v3RPY[0])-sin(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]), -cos(v3RPY[1])*sin(v3RPY[0]), (-sin(v3RPY[2])*sin(v3RPY[0])-cos(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]))*v3T[0]+(cos(v3RPY[2])*sin(v3RPY[0])-sin(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]))*v3T[1]-cos(v3RPY[1])*cos(v3RPY[0])*v3T[2], -cos(v3RPY[2])*cos(v3RPY[1])*sin(v3RPY[0])*v3T[0]-sin(v3RPY[2])*cos(v3RPY[1])*sin(v3RPY[0])*v3T[1]+sin(v3RPY[1])*sin(v3RPY[0])*v3T[2], (cos(v3RPY[2])*cos(v3RPY[0])+sin(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]))*v3T[0]+(sin(v3RPY[2])*cos(v3RPY[0])-cos(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]))*v3T[1],

-sin(v3RPY[2])*sin(v3RPY[0])-cos(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]), cos(v3RPY[2])*sin(v3RPY[0])-sin(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]), -cos(v3RPY[1])*cos(v3RPY[0]), (-sin(v3RPY[2])*cos(v3RPY[0])+cos(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]))*v3T[0]+(cos(v3RPY[2])*cos(v3RPY[0])+sin(v3RPY[2])*sin(v3RPY[1])*sin(v3RPY[0]))*v3T[1]+cos(v3RPY[1])*sin(v3RPY[0])*v3T[2], -cos(v3RPY[2])*cos(v3RPY[1])*cos(v3RPY[0])*v3T[0]-sin(v3RPY[2])*cos(v3RPY[1])*cos(v3RPY[0])*v3T[1]+sin(v3RPY[1])*cos(v3RPY[0])*v3T[2], (-cos(v3RPY[2])*sin(v3RPY[0])+sin(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]))*v3T[0]+(-sin(v3RPY[2])*sin(v3RPY[0])-cos(v3RPY[2])*sin(v3RPY[1])*cos(v3RPY[0]))*v3T[1]);

  return J;
}
 

/*TooN::SO3<> PackSO3_YPR(float yaw, float pitch, float roll)
{
  WrapTo2Pi(yaw);
  WrapTo2Pi(pitch);
  WrapTo2Pi(roll);

  const float cy = cos(yaw);
  const float sy = sin(yaw);
  const float cp = cos(pitch);
  const float sp = sin(pitch);
  const float cr = cos(roll);
  const float sr = sin(roll);

  const TooN::Matrix<3,3> R = TooN::Data(
          cy*cp,      cy*sp*sr-sy*cr,     cy*sp*cr+sy*sr,
          sy*cp,      sy*sp*sr+cy*cr,     sy*sp*cr-cy*sr,
          -sp,        cp*sr,              cp*cr
          );
  return TooN::SO3<>(R);
}

TooN::Matrix<3,3> RodriguesExp(const TooN::Vector<3> v3W, const double dA, const double dB)
{
  Matrix<3,3> m33R;
  {
    const double wx2 = v3W[0]*v3W[0];
    const double wy2 = v3W[1]*v3W[1];
    const double wz2 = v3W[2]*v3W[2];
    m33R(0,0) = 1.0 - dB*(wy2 + wz2);
    m33R(1,1) = 1.0 - dB*(wx2 + wz2);
    m33R(2,2) = 1.0 - dB*(wx2 + wy2);
  }
  {
    const double a = dA*v3W[2];
    const double b = dB*(v3W[0]*v3W[1]);
    m33R(0,1) = b - a;
    m33R(1,0) = b + a;
  }
  {
    const double a = dA*v3W[1];
    const double b = dB*(v3W[0]*v3W[2]);
    m33R(0,2) = b + a;
    m33R(2,0) = b - a;
  }
  {
    const double a = dA*v3W[0];
    const double b = dB*(v3W[1]*v3W[2]);
    m33R(1,2) = b - a;
    m33R(2,1) = b + a;
  }
  return m33R;
}

// Exponentiate a vector in the Lie algebra to generate a new SO3 (a 3x3 rotation matrix).
TooN::Matrix<3,3> expYPR(const TooN::Vector<3> &v3W)
{
  static const double dOne6th = 1.0/6.0;
  const double dThetaSq = v3W*v3W;
  const double dTheta = sqrt(dThetaSq);
  double dA, dB;

  //Use a Taylor series expansion near zero. This is required for
  //accuracy, since sin t / t and (1-cos t)/t^2 are both 0/0.
  if(dThetaSq < 1e-8) 
  {
    dA = 1.0 - dOne6th * dThetaSq;
    dB = 0.5;
  } 
  else
  {
    if(dThetaSq < 1e-6) 
    {
      dB = 0.5 - 0.25 * dOne6th * dThetaSq;
      dA = 1.0 - dThetaSq * dOne6th*(1.0 - 0.05*dThetaSq);
    } 
    else 
    {
      const double dThetaInv = 1.0/dTheta;
      dA = sin(dTheta) * dThetaInv;
      dB = (1 - cos(dTheta)) * (dThetaInv * dThetaInv);
    }
  }

  return RodriguesExp(v3W, dA, dB);
}

// Jacobian of SO3 logarithmic map wrt. R
inline TooN::Matrix<3,9> Jacobian_lnR_dR(const TooN::Matrix<3,3> & R)
{
  double d = 0.5*(R(0,0)+R(1,1)+R(2,2)-1);

  TooN::Vector<3> a ;
  TooN::Matrix<3,3> B;
  if(d>0.99999)
  {
    a = TooN::makeVector(0,0,0);
    B = -0.5*TooN::Identity;
  }
  else
  {
    double theta = acos(d);
    double d2 = d*d;
    double sq = sqrt(1-d2);
    TooN::Vector<3> deltaR = TooN::makeVector(R(2,1)-R(1,2),R(0,2)-R(2,0),R(1,0)-R(0,1));
    a = (d*theta-sq)/(4*std::pow(sq,3))*deltaR;
    B = -theta/(2*sq)*TooN::Identity(3);
  }

  TooN::Matrix<3,9> J;
  J.T()[0] = a;
  J.T()[1] = -B.T()[2];
  J.T()[2] = B.T()[1];
  J.T()[3] = B.T()[2];
  J.T()[4] = a;
  J.T()[5] = -B.T()[0];
  J.T()[6] = -B.T()[1];
  J.T()[7] = B.T()[0];
  J.T()[8] = a;

  return J;
}*/

#endif
